/******************************************************************************
* AIR SYSTEMS LABORATORY
* AEROSPACE ENGINEERING
* UNIVERISTY OF TEXAS AT AUSTIN
* UAV FLIGHT SOFTWARE 
* DESCRIPTION:
*   
*
* SOURCE: 
* Orignial: 08/14/2008  Jorge Alvarez joroge@gmail.com
* Modified: 03/20/2009	Sergio García-Nieto sergarro@isa.upv.es
* $Id: telemetry_thread.c 4 2009-07-21 17:12:11Z joroge@gmail.com $
*************************************************************************************/
#include <time.h>
#include <stdio.h>
#include <fcntl.h>
#include <termios.h>
#include <string.h>
#include <pthread.h>
#include <stdlib.h>
#include <stdbool.h>
#include <unistd.h>

#include "globaldefs.h"
#include "utilities.h"
#include "serial.h"
#include "telemetry_thread.h"



void *telemetry()
{
	
	struct timespec t0,t1,t0_ant,wait,next_activation;
	time_t tstamp;
	struct 	tm *tstampPtr;
	_uint64 auxtime;
	int 	  	sPort1;
	int 		nbytes=0;
	byte		stateAux[150]={NULL,};
	byte		pidAux[100]={0,};
	byte		machineAux[100]={0,};
	byte		groundMsg[100]={0,};
	byte		optionMsg[3]={NULL,};
	byte 		timestamp[10]={NULL,};
	byte		tAux[2];
	
	struct 		timespec delay;
	struct 		imuShort iSB[500];
	struct 		gpsAdc gAB[250];
	int			fd1,fd2;
	int			nbytes1,nbytes2;
	float		Alpha, Beta;
	int 		num;
	int			option=0;
	int			lastIMU,sizeIMU;
	int			lastGA,sizeGA;
	int			actualWayPointAux;
	int			numTot;
	float		latAux,lonAux;
	// Data Packets Declaration
	static byte	stateVector[150]={NULL,};
	static byte	pidRollParam[100]={NULL,};
	static byte	pidPitchParam[100]={NULL,};
	static byte	pidYawParam[100]={NULL,};
	static byte	pidHeadingParam[100]={NULL,};
	static byte	pidVelocityParam[100]={NULL,};
	static byte	pidAltitudeParam[100]={NULL,};
	static byte machineStateParam[100]={NULL,};
	

	delay.tv_sec=1;
	delay.tv_nsec=0;
	
	// Open de FIFOs
	fd1 = open(fifoAttiTele,O_RDONLY | O_NONBLOCK);
	fd2 = open(fifoNavTele,O_RDONLY | O_NONBLOCK);
	sizeIMU = (sizeof(struct imuShort));
	sizeGA = (sizeof(struct gpsAdc));	
	
	// Open Serial Port 1
	sPort1 = open_serial(SERIAL_PORT3,BAUDRATE_115200);	
	
	nanosleep(&delay,NULL);
	clock_gettime(CLOCK_REALTIME, &t0_ant);
	clock_gettime(CLOCK_REALTIME, &t1);
	printf ("Telemetry Task on-line.\n");	
	while(1) 
	{
		// Loop time claculations
		clock_gettime (CLOCK_REALTIME, &t0);
		next_activation=sum_tiempos(t0,Ts_telemetry); 
		//printf ("Telemetry Running.\n");	
		// Read Angles from Gimble Encoders
		Alpha = 30;
		Beta = 50;

		// Download FIFOs DATA
		nbytes1 = read(fd1,&iSB,sizeIMU*75);
		nbytes2 = read(fd2,&gAB,sizeGA*25);
		lastIMU = (int)(nbytes1/sizeIMU)-1;
		lastGA = (int)(nbytes2/sizeGA)-1;
		//printf ("Last IMU: %d.\n",lastIMU);
		//printf ("Last Ga: %d.\n",lastGA);
		//printf ("Step 2.\n");
		//printf("Telemetry Running \n");
		// FORMATTING AND SENDING DATA
		// Vector State Packet
		sprintf(stateVector,"UVS");
		sprintf(stateAux,"%08X%08X%08X%08X%08X%08X%08X%08X%08X%08X%08X%08X%08X%s",*(int *)&iSB[lastIMU].roll,*(int *)&iSB[lastIMU].pitch,*(int *)&iSB[lastIMU].yaw,*(int *)&gAB[lastGA].lat,*(int *)&gAB[lastGA].lon,*(int *)&gAB[lastGA].alt_msl,*(int *)&gAB[lastGA].ve,*(int *)&gAB[lastGA].vn,*(int *)&gAB[lastGA].vd,*(int *)&gAB[lastGA].airSpeed,*(int *)&gAB[lastGA].pAltitude,*(int *)&Beta,*(int *)&Alpha,timestamp);
		strcat(stateVector,stateAux);
		createChecksum(stateVector,119);
		nbytes = write(sPort1,(char*)&stateVector, strlen(stateVector));

		// Roll PID Packet
		sprintf(pidRollParam,"UVPR");
		pthread_mutex_lock(&mutexPidLowLevel);
		sprintf(pidAux,"%08X%08X%08X%08X%08X%08X%08X%08X%08X",*(int *)&pidRoll.Signals.R,*(int *)&pidRoll.Signals.u,*(int *)&pidRoll.Signals.y,*(int *)&pidRoll.Par.K,*(int *)&pidRoll.Par.Td,*(int *)&pidRoll.Par.Ti,*(int *)&pidRoll.Par.ulow,*(int *)&pidRoll.Par.uhigh,*(int *)&pidRoll.Par.Ts);
		pthread_mutex_unlock(&mutexPidLowLevel);
		strcat(pidRollParam,pidAux);
		createChecksum(pidRollParam,78);
		nbytes = write(sPort1,(char*)&pidRollParam, strlen(pidRollParam));

		// Pitch PID Packet
		sprintf(pidPitchParam,"UVPP");
		pthread_mutex_lock(&mutexPidLowLevel);
		sprintf(pidAux,"%08X%08X%08X%08X%08X%08X%08X%08X%08X",*(int *)&pidPitch.Signals.R,*(int *)&pidPitch.Signals.u,*(int *)&pidPitch.Signals.y,*(int *)&pidPitch.Par.K,*(int *)&pidPitch.Par.Td,*(int *)&pidPitch.Par.Ti,*(int *)&pidPitch.Par.ulow,*(int *)&pidPitch.Par.uhigh,*(int *)&pidPitch.Par.Ts);
		pthread_mutex_unlock(&mutexPidLowLevel);
		strcat(pidPitchParam,pidAux);
		createChecksum(pidPitchParam,78);
		nbytes = write(sPort1,(char*)&pidPitchParam, strlen(pidPitchParam));

		// Yaw PID Packet
		sprintf(pidYawParam,"UVPY");
		pthread_mutex_lock(&mutexPidLowLevel);
		sprintf(pidAux,"%08X%08X%08X%08X%08X%08X%08X%08X%08X",*(int *)&pidYaw.Signals.R,*(int *)&pidYaw.Signals.u,*(int *)&pidYaw.Signals.y,*(int *)&pidYaw.Par.K,*(int *)&pidYaw.Par.Td,*(int *)&pidYaw.Par.Ti,*(int *)&pidYaw.Par.ulow,*(int *)&pidYaw.Par.uhigh,*(int *)&pidYaw.Par.Ts);
		pthread_mutex_unlock(&mutexPidLowLevel);
		strcat(pidYawParam,pidAux);
		createChecksum(pidYawParam,78);
		nbytes = write(sPort1,(char*)&pidYawParam, strlen(pidYawParam));

		// Altitude PID Packet
		sprintf(pidAltitudeParam,"UVPA");
		pthread_mutex_lock(&mutexPidHighLevel);
		sprintf(pidAux,"%08X%08X%08X%08X%08X%08X%08X%08X%08X",*(int *)&pidAltitude.Signals.R,*(int *)&pidAltitude.Signals.u,*(int *)&pidAltitude.Signals.y,*(int *)&pidAltitude.Par.K,*(int *)&pidAltitude.Par.Td,*(int *)&pidAltitude.Par.Ti,*(int *)&pidAltitude.Par.ulow,*(int *)&pidAltitude.Par.uhigh,*(int *)&pidAltitude.Par.Ts);
		pthread_mutex_unlock(&mutexPidHighLevel);
		strcat(pidAltitudeParam,pidAux);
		createChecksum(pidAltitudeParam,78);
		nbytes = write(sPort1,(char*)&pidAltitudeParam, strlen(pidAltitudeParam));

		// Heading PID Packet
		sprintf(pidHeadingParam,"UVPH");
		pthread_mutex_lock(&mutexPidHighLevel);
		sprintf(pidAux,"%08X%08X%08X%08X%08X%08X%08X%08X%08X",*(int *)&pidHeading.Signals.R,*(int *)&pidHeading.Signals.u,*(int *)&pidHeading.Signals.y,*(int *)&pidHeading.Par.K,*(int *)&pidHeading.Par.Td,*(int *)&pidHeading.Par.Ti,*(int *)&pidHeading.Par.ulow,*(int *)&pidHeading.Par.uhigh,*(int *)&pidHeading.Par.Ts);
		pthread_mutex_unlock(&mutexPidHighLevel);
		strcat(pidHeadingParam,pidAux);
		createChecksum(pidHeadingParam,78);
		nbytes = write(sPort1,(char*)&pidHeadingParam, strlen(pidHeadingParam));

		// Velocity PID Packet
		sprintf(pidVelocityParam,"UVPV");
		pthread_mutex_lock(&mutexPidHighLevel);
		sprintf(pidAux,"%08X%08X%08X%08X%08X%08X%08X%08X%08X",*(int *)&pidVelocity.Signals.R,*(int *)&pidVelocity.Signals.u,*(int *)&pidVelocity.Signals.y,*(int *)&pidVelocity.Par.K,*(int *)&pidVelocity.Par.Td,*(int *)&pidVelocity.Par.Ti,*(int *)&pidVelocity.Par.ulow,*(int *)&pidVelocity.Par.uhigh,*(int *)&pidVelocity.Par.Ts);
		pthread_mutex_unlock(&mutexPidHighLevel);
		strcat(pidVelocityParam,pidAux);
		createChecksum(pidVelocityParam,78);
		nbytes = write(sPort1,(char*)&pidVelocityParam, strlen(pidVelocityParam));

		// State Machine Packet
		sprintf(machineStateParam,"UVM");
		pthread_mutex_lock(&mutexGlobalVar);
		actualWayPointAux = actualWayPoint+1;
		numTot = totalWayPoints;
		latAux = flightPlan[actualWayPoint].latRef;
		lonAux = flightPlan[actualWayPoint].lonRef;	
		pthread_mutex_unlock(&mutexGlobalVar);
		pthread_mutex_lock(&mutexMachineState);
		sprintf(machineAux,"%04X%04X%04X%04X%04X%04X%04X%08X%08X",*(int *)&X1_idle,*(int *)&X2_takeoff,*(int *)&X3_navigation,*(int *)&X4_landing,*(int *)&semiAutomatic,*(int *)&numTot,*(int *)&actualWayPointAux,*(int *)&latAux,*(int *)&lonAux);
		pthread_mutex_unlock(&mutexMachineState);
		strcat(machineStateParam,machineAux);
		createChecksum(machineStateParam,49);
		nbytes = write(sPort1,(char*)&machineStateParam, strlen(machineStateParam));


		// Read Messeges from Ground Station
		nbytes=readcond(sPort1,groundMsg,25,25,1,1);
		//nbytes=read(sPort1,groundMsg,25);
		if (nbytes==25)
		{   
			strncpy(optionMsg,groundMsg,3);
			option = (int)(optionMsg[0]+optionMsg[1]+optionMsg[2]);
			//printf("%d\n",option); 
			switch (option)
			{
			// UVM Packet
			case 248:
				if(checksumCheckTele(groundMsg,GROUNDMSG_PACKET_LENGTH)==TRUE)
										 {	// Update MachineState from GroundStation
											num = (int)(groundMsg[3]);
											printf("State Machin %d\n",num);
											switch(num)
											{
											case 8:
												pthread_mutex_lock(&mutexMachineState);
												event_0 = 0;
												event_1 = 0;
												event_2 = 0;
												event_3 = 1;
												event_4 = 0;
												event_5 = 0;
												pthread_mutex_unlock(&mutexMachineState);	
												break;
											case 4:
												pthread_mutex_lock(&mutexMachineState);
												event_0 = 0;
												event_1 = 0;
												event_2 = 1;
												event_3 = 0;
												event_4 = 0;
												event_5 = 0;
												pthread_mutex_unlock(&mutexMachineState);
												break;
											}
											// Update Automatic-SemiAutomatic from GroundStation
											num = (int)(groundMsg[4]);
											if (num==2)
											{
												pthread_mutex_lock(&mutexMachineState);
												semiAutomatic = 1;	
												pthread_mutex_unlock(&mutexMachineState);
											}
											if (num==1)
											{
												pthread_mutex_lock(&mutexMachineState);
												semiAutomatic = 0;	
												pthread_mutex_unlock(&mutexMachineState);
											}
											//num = (int)(groundMsg[4]);
											//printf("Autonomo %d\n",num);
											
											// Update actualWayPoint from GroundStation
											num = (int)(groundMsg[5]);
											pthread_mutex_lock(&mutexGlobalVar);
											if ((num>0)&&(num<=(totalWayPoints)))
												actualWayPoint = num-1;
											pthread_mutex_unlock(&mutexGlobalVar);	
											
											// Clear all Waypoints
											num = (int)(groundMsg[7]);
											if (num==1)
											{
												pthread_mutex_lock(&mutexGlobalVar);
													totalWayPoints = 0;
													actualWayPoint = 0;
												pthread_mutex_unlock(&mutexGlobalVar);
											}
										 }
			break;	
			// UVW Packet
			case 258:

				if(checksumCheckTele(groundMsg,GROUNDMSG_PACKET_LENGTH)==TRUE)
						 {
							num = (int)(floatFromBytesLE(&groundMsg[19]))-1;
							pthread_mutex_lock(&mutexGlobalVar);
							flightPlan[num].numPoint = (int)(floatFromBytesLE(&groundMsg[19]));
							flightPlan[num].latRef = (float)(floatFromBytesLE(&groundMsg[3]));
							flightPlan[num].lonRef = (float)(floatFromBytesLE(&groundMsg[7]));
							flightPlan[num].altRef = (float)(floatFromBytesLE(&groundMsg[11]));
							flightPlan[num].velRef = (float)(floatFromBytesLE(&groundMsg[15]));
							if (flightPlan[num].numPoint>totalWayPoints)
								totalWayPoints = flightPlan[num].numPoint;
							pthread_mutex_unlock(&mutexGlobalVar);
							
						 }
			break;
			// UVP Packet
			case 251:

				if(checksumCheckTele(groundMsg,GROUNDMSG_PACKET_LENGTH)==TRUE)
						 {
					        pthread_mutex_lock(&mutexPidLowLevel);
							pidPitch.Par.K = (float)(floatFromBytesLE(&groundMsg[3]));
							pidPitch.Par.Ti = (float)(floatFromBytesLE(&groundMsg[7]));
							pidPitch.Par.Td = (float)(floatFromBytesLE(&groundMsg[11]));
							//pidPitch.Par.ulow = (float)(floatFromBytesLE(&groundMsg[15]));
							//pidPitch.Par.uhigh = (int)(floatFromBytesLE(&groundMsg[19]));
							pthread_mutex_unlock(&mutexPidLowLevel);
						 }

			break;
			// UVR Packet
			case 253:

				if(checksumCheckTele(groundMsg,GROUNDMSG_PACKET_LENGTH)==TRUE)
						 {
			                pthread_mutex_lock(&mutexPidLowLevel);					
							pidRoll.Par.K = (float)(floatFromBytesLE(&groundMsg[3]));
							pidRoll.Par.Ti = (float)(floatFromBytesLE(&groundMsg[7]));
							pidRoll.Par.Td = (float)(floatFromBytesLE(&groundMsg[11]));
							//pidRoll.Par.ulow = (float)(floatFromBytesLE(&groundMsg[15]));
							//pidRoll.Par.uhigh = (int)(floatFromBytesLE(&groundMsg[19]));
							pthread_mutex_unlock(&mutexPidLowLevel);
						 }

			break;
			// UVY Packet
			case 260:

				if(checksumCheckTele(groundMsg,GROUNDMSG_PACKET_LENGTH)==TRUE)
						 {
			                pthread_mutex_lock(&mutexPidLowLevel);
							pidYaw.Par.K = (float)(floatFromBytesLE(&groundMsg[3]));
							pidYaw.Par.Ti = (float)(floatFromBytesLE(&groundMsg[7]));
							pidYaw.Par.Td = (float)(floatFromBytesLE(&groundMsg[11]));
							//pidYaw.Par.ulow = (float)(floatFromBytesLE(&groundMsg[15]));
							//pidYaw.Par.uhigh = (int)(floatFromBytesLE(&groundMsg[19]));
							pthread_mutex_unlock(&mutexPidLowLevel);
						 }

			break;
			// UVA Packet
			case 236:

				if(checksumCheckTele(groundMsg,GROUNDMSG_PACKET_LENGTH)==TRUE)
						 {
			        		pthread_mutex_lock(&mutexPidHighLevel);
							pidAltitude.Par.K = (float)(floatFromBytesLE(&groundMsg[3]));
							pidAltitude.Par.Ti = (float)(floatFromBytesLE(&groundMsg[7]));
							pidAltitude.Par.Td = (float)(floatFromBytesLE(&groundMsg[11]));
							//pidAltitude.Par.ulow = (float)(floatFromBytesLE(&groundMsg[15]));
							//pidAltitude.Par.uhigh = (int)(floatFromBytesLE(&groundMsg[19]));
							pthread_mutex_unlock(&mutexPidHighLevel);
						 }

			break;
			// UVH Packet
			case 243:

				if(checksumCheckTele(groundMsg,GROUNDMSG_PACKET_LENGTH)==TRUE)
						 {
							pthread_mutex_lock(&mutexPidHighLevel);
							pidHeading.Par.K = (float)(floatFromBytesLE(&groundMsg[3]));
							pidHeading.Par.Ti = (float)(floatFromBytesLE(&groundMsg[7]));
							pidHeading.Par.Td = (float)(floatFromBytesLE(&groundMsg[11]));
							//pidHeading.Par.ulow = (float)(floatFromBytesLE(&groundMsg[15]));
							//pidHeading.Par.uhigh = (int)(floatFromBytesLE(&groundMsg[19]));
							pthread_mutex_unlock(&mutexPidHighLevel);
						 }

			break;
			// UVV Packet
			case 257:
				if(checksumCheckTele(groundMsg,GROUNDMSG_PACKET_LENGTH)==TRUE)
						 {
							pthread_mutex_lock(&mutexPidHighLevel);
							pidVelocity.Par.K = (float)(floatFromBytesLE(&groundMsg[3]));
							pidVelocity.Par.Ti = (float)(floatFromBytesLE(&groundMsg[7]));
							pidVelocity.Par.Td = (float)(floatFromBytesLE(&groundMsg[11]));
							//pidVelocity.Par.ulow = (float)(floatFromBytesLE(&groundMsg[15]));
							//pidVelocity.Par.uhigh = (int)(floatFromBytesLE(&groundMsg[19]));
							pthread_mutex_unlock(&mutexPidHighLevel);
						 }

			break;

			}


		}


		// Print the results
		//if(v) printf("Inicialization--> WCCT: %3d Period: %3d\n",((int)WCCT.tv_nsec)/prec,  ((int)t0.tv_nsec-(int)t0_ant.tv_nsec)/prec);
		//if(v) printf("T %3d %3d \n",((int)WCCT.tv_nsec)/prec,  ((int)Period.tv_nsec)/prec);
		
		// Request Angles from Encoders
		
		// Generate Time Stamp for Angles
		tstamp = time(NULL);
		tstampPtr = gmtime(&tstamp);
		strftime(timestamp,10,"%T",tstampPtr);
		clock_gettime (CLOCK_REALTIME, &t1);
		strcat(timestamp,":");
		sprintf(tAux,"%d",((int)t1.tv_nsec/100000000));
		strcat(timestamp,tAux);
		
		// Wait until the next activation
		clock_gettime (CLOCK_REALTIME, &t1);
		if (timespec2nsec(&next_activation)>timespec2nsec(&t1))
		{
		    auxtime = (timespec2nsec(&next_activation)-timespec2nsec(&t1));
			nsec2timespec(&wait,auxtime);
			nanosleep(&wait, NULL);
		}
	}
	// Close Port
	close(sPort1);
	return(NULL);
}
/***************************************************************************************
 *check the checksum of the Telemetry packet
 ***************************************************************************************/
int checksumCheckTele(byte* buffer, int packet_len)
{
   short     	 i=0;
   byte			 ck_a=0,ck_b=0;
   byte			 ck_a_p=0,ck_b_p=0;
 
   for(i=2;i<packet_len-2;i++)
    {
   		ck_a=ck_a + buffer[i];
   		ck_b=ck_b + ck_a;
   		}
   ck_a_p = buffer[packet_len-1];
   ck_b_p = buffer[packet_len-2];
   //printf("Checksum %c\n",ck_a_p);
   //printf("Checksum %c\n",ck_b_p);
   //printf("Checksum %c\n",ck_a);
   //printf("Checksum %c\n",ck_b);
   if ((ck_a_p==ck_a)& (ck_b_p==ck_b))
   	return    TRUE;
   else
 	return    FALSE;
}
